#include "local_planner/collision_checker.h"

namespace Planner
{
    void CollisionChecker::setParams(double dis, int num, double hangle, double vangle)
    {
        secure_dis_ = dis;
        iter_num_ = num;
        vertical_angle_ = vangle;
        horizontal_angle_ = hangle;
    }

    void CollisionChecker::setInputCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr &tree)
    {
        cloud_map_ = cloud;
        kdtree_ = tree;
    }

    void CollisionChecker::setCurrPose(const Vector3d &pose, const Quaterniond &orien)
    {
        curr_pose_ = pose;
        curr_orien_ = orien;
    }

    bool CollisionChecker::isFeasible(const Vector3d &waypt, double &dis)
    {
        bool flag = nearSearch(waypt);
        dis = near_dis_;
        if (dis >= secure_dis_)
            return true;

        return false;
    }

    bool CollisionChecker::nearSearch(const Vector3d &pt)
    {
        pcl::PointXYZ searchPoint;
        searchPoint.x = pt[0];
        searchPoint.y = pt[1];
        searchPoint.z = pt[2];
        std::vector<int> pointIdxNKNSearch(1);
        std::vector<float> pointNKNSquaredDistance(1);
        if (kdtree_->nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
        {
            near_pt_[0] = cloud_map_->points[pointIdxNKNSearch[0]].x;
            near_pt_[1] = cloud_map_->points[pointIdxNKNSearch[0]].y;
            near_pt_[2] = cloud_map_->points[pointIdxNKNSearch[0]].z;
            near_dis_ = std::sqrt(pointNKNSquaredDistance[0]);
            return true;
        }
        std::cout << "Cannot find nearest point\n";
        return false;
    }

    bool CollisionChecker::insideFOV(const Vector3d &pt)
    {
        // 当前位置雷达坐标系对应的x\y\z轴方向向量
        Vector3d direction_x = curr_orien_._transformVector(Vector3d(1, 0, 0));
        direction_x.normalize();
        Vector3d direction_z(0, 0, 1);

        // 计算与x\z方向的夹角
        Vector3d direction_replan = pt - curr_pose_;
        double cosz = direction_replan.normalized().transpose() * direction_z;
        double anglez = abs(90.f - RAD2DEG(acos(cosz)));
        if (anglez > vertical_angle_)
        {
            std::cout << "out of FOV, anglez: " << anglez << std::endl;
            return false;
        }
        Vector3d direction_xy = direction_replan - cosz * direction_z;
        double cosx = direction_xy.normalized().transpose() * direction_x.normalized();
        double anglex = RAD2DEG(acos(cosx));
        if (anglex > horizontal_angle_)
        {
            std::cout << "out fo FOV, anglex: " << anglex << std::endl;
            return false;
        }
        return true;
    }

    double CollisionChecker::calcDis(const Vector3d &pt)
    {
        pcl::PointXYZ searchPoint;
        searchPoint.x = pt[0];
        searchPoint.y = pt[1];
        searchPoint.z = pt[2];
        std::vector<int> pointIdxNKNSearch(1);
        std::vector<float> pointNKNSquaredDistance(1);
        if (kdtree_->nearestKSearch(searchPoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
        {
            double dis = pow(pointNKNSquaredDistance[0], 0.5);
            return dis;
        }
        cout << "nearest search failed\n";
        return -1;
    }

    // 用梯度和邻近点效果都不好，试一下树搜索
    bool CollisionChecker::replanByTree(const Vector3d &waypt, const Vector3d &direction, Vector3d &revise_pt)
    {
        // 根据direction计算direction_x和y, 让direction_x与障碍物反向
        bool flag = false;
        int step = 0;
        revise_pt = waypt;
        Vector3d direction_buff, direction_x, direction_y;
        double cos_projection, move_dis;
        while (step < iter_num_ && !flag)
        {
            direction_buff = revise_pt - near_pt_;
            cos_projection = direction_buff.transpose() * direction.normalized();
            direction_x = direction_buff - cos_projection * direction.normalized();
            direction_y = direction.cross(direction_x);
            direction_x.normalize();
            direction_y.normalize();
            move_dis = secure_dis_ - near_dis_ + 0.1;

            double best_dis = near_dis_;
            Vector3d best_loc = revise_pt;
            // 尝试一下各个方向，以30度为间隔，找出之后据障碍物最远的
            for (size_t i = 0; i < 5; i++)
            {
                double angle = (-60.0 + i * 30.0) * M_PI / 180.0;
                Vector3d curr_loc = revise_pt + move_dis * (direction_x * cos(angle) + direction_y * sin(angle));
                double curr_dis = calcDis(curr_loc);
                if (curr_dis > best_dis)
                {
                    best_dis = curr_dis;
                    best_loc = curr_loc;
                }
            }
            revise_pt = best_loc;
            nearSearch(revise_pt);
            if (near_dis_ > secure_dis_)
                flag = true;
            cout << "iteration " << step << ": revised to (" << revise_pt[0] << ", "
                 << revise_pt[1] << ", " << revise_pt[2] << "), distance is: " << near_dis_ << endl;
            step++;
        }
        return flag && insideFOV(revise_pt);
    }
} // namespace Planner